// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_STABLENORM_H
#define EIGEN_STABLENORM_H

namespace Eigen {

namespace internal {

template<typename ExpressionType, typename Scalar>
inline void
stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale)
{
	Scalar maxCoeff = bl.cwiseAbs().maxCoeff();

	if (maxCoeff > scale) {
		ssq = ssq * numext::abs2(scale / maxCoeff);
		Scalar tmp = Scalar(1) / maxCoeff;
		if (tmp > NumTraits<Scalar>::highest()) {
			invScale = NumTraits<Scalar>::highest();
			scale = Scalar(1) / invScale;
		} else if (maxCoeff > NumTraits<Scalar>::highest()) // we got a INF
		{
			invScale = Scalar(1);
			scale = maxCoeff;
		} else {
			scale = maxCoeff;
			invScale = tmp;
		}
	} else if (maxCoeff != maxCoeff) // we got a NaN
	{
		scale = maxCoeff;
	}

	// TODO if the maxCoeff is much much smaller than the current scale,
	// then we can neglect this sub vector
	if (scale > Scalar(0)) // if scale==0, then bl is 0
		ssq += (bl * invScale).squaredNorm();
}

template<typename VectorType, typename RealScalar>
void
stable_norm_impl_inner_step(const VectorType& vec, RealScalar& ssq, RealScalar& scale, RealScalar& invScale)
{
	typedef typename VectorType::Scalar Scalar;
	const Index blockSize = 4096;

	typedef typename internal::nested_eval<VectorType, 2>::type VectorTypeCopy;
	typedef typename internal::remove_all<VectorTypeCopy>::type VectorTypeCopyClean;
	const VectorTypeCopy copy(vec);

	enum
	{
		CanAlign =
			((int(VectorTypeCopyClean::Flags) & DirectAccessBit) ||
			 (int(internal::evaluator<VectorTypeCopyClean>::Alignment) > 0) // FIXME Alignment)>0 might not be enough
			 ) &&
			(blockSize * sizeof(Scalar) * 2 < EIGEN_STACK_ALLOCATION_LIMIT) &&
			(EIGEN_MAX_STATIC_ALIGN_BYTES >
			 0) // if we cannot allocate on the stack, then let's not bother about this optimization
	};
	typedef typename internal::conditional<
		CanAlign,
		Ref<const Matrix<Scalar, Dynamic, 1, 0, blockSize, 1>, internal::evaluator<VectorTypeCopyClean>::Alignment>,
		typename VectorTypeCopyClean::ConstSegmentReturnType>::type SegmentWrapper;
	Index n = vec.size();

	Index bi = internal::first_default_aligned(copy);
	if (bi > 0)
		internal::stable_norm_kernel(copy.head(bi), ssq, scale, invScale);
	for (; bi < n; bi += blockSize)
		internal::stable_norm_kernel(
			SegmentWrapper(copy.segment(bi, numext::mini(blockSize, n - bi))), ssq, scale, invScale);
}

template<typename VectorType>
typename VectorType::RealScalar
stable_norm_impl(const VectorType& vec, typename enable_if<VectorType::IsVectorAtCompileTime>::type* = 0)
{
	using std::abs;
	using std::sqrt;

	Index n = vec.size();

	if (n == 1)
		return abs(vec.coeff(0));

	typedef typename VectorType::RealScalar RealScalar;
	RealScalar scale(0);
	RealScalar invScale(1);
	RealScalar ssq(0); // sum of squares

	stable_norm_impl_inner_step(vec, ssq, scale, invScale);

	return scale * sqrt(ssq);
}

template<typename MatrixType>
typename MatrixType::RealScalar
stable_norm_impl(const MatrixType& mat, typename enable_if<!MatrixType::IsVectorAtCompileTime>::type* = 0)
{
	using std::sqrt;

	typedef typename MatrixType::RealScalar RealScalar;
	RealScalar scale(0);
	RealScalar invScale(1);
	RealScalar ssq(0); // sum of squares

	for (Index j = 0; j < mat.outerSize(); ++j)
		stable_norm_impl_inner_step(mat.innerVector(j), ssq, scale, invScale);
	return scale * sqrt(ssq);
}

template<typename Derived>
inline typename NumTraits<typename traits<Derived>::Scalar>::Real
blueNorm_impl(const EigenBase<Derived>& _vec)
{
	typedef typename Derived::RealScalar RealScalar;
	using std::abs;
	using std::pow;
	using std::sqrt;

	// This program calculates the machine-dependent constants
	// bl, b2, slm, s2m, relerr overfl
	// from the "basic" machine-dependent numbers
	// nbig, ibeta, it, iemin, iemax, rbig.
	// The following define the basic machine-dependent constants.
	// For portability, the PORT subprograms "ilmaeh" and "rlmach"
	// are used. For any specific computer, each of the assignment
	// statements can be replaced
	static const int ibeta = std::numeric_limits<RealScalar>::radix; // base for floating-point numbers
	static const int it = NumTraits<RealScalar>::digits();			 // number of base-beta digits in mantissa
	static const int iemin = NumTraits<RealScalar>::min_exponent();	 // minimum exponent
	static const int iemax = NumTraits<RealScalar>::max_exponent();	 // maximum exponent
	static const RealScalar rbig = NumTraits<RealScalar>::highest(); // largest floating-point number
	static const RealScalar b1 =
		RealScalar(pow(RealScalar(ibeta), RealScalar(-((1 - iemin) / 2)))); // lower boundary of midrange
	static const RealScalar b2 =
		RealScalar(pow(RealScalar(ibeta), RealScalar((iemax + 1 - it) / 2))); // upper boundary of midrange
	static const RealScalar s1m =
		RealScalar(pow(RealScalar(ibeta), RealScalar((2 - iemin) / 2))); // scaling factor for lower range
	static const RealScalar s2m =
		RealScalar(pow(RealScalar(ibeta), RealScalar(-((iemax + it) / 2)))); // scaling factor for upper range
	static const RealScalar eps = RealScalar(pow(double(ibeta), 1 - it));
	static const RealScalar relerr = sqrt(eps); // tolerance for neglecting asml

	const Derived& vec(_vec.derived());
	Index n = vec.size();
	RealScalar ab2 = b2 / RealScalar(n);
	RealScalar asml = RealScalar(0);
	RealScalar amed = RealScalar(0);
	RealScalar abig = RealScalar(0);

	for (Index j = 0; j < vec.outerSize(); ++j) {
		for (typename Derived::InnerIterator iter(vec, j); iter; ++iter) {
			RealScalar ax = abs(iter.value());
			if (ax > ab2)
				abig += numext::abs2(ax * s2m);
			else if (ax < b1)
				asml += numext::abs2(ax * s1m);
			else
				amed += numext::abs2(ax);
		}
	}
	if (amed != amed)
		return amed; // we got a NaN
	if (abig > RealScalar(0)) {
		abig = sqrt(abig);
		if (abig > rbig) // overflow, or *this contains INF values
			return abig; // return INF
		if (amed > RealScalar(0)) {
			abig = abig / s2m;
			amed = sqrt(amed);
		} else
			return abig / s2m;
	} else if (asml > RealScalar(0)) {
		if (amed > RealScalar(0)) {
			abig = sqrt(amed);
			amed = sqrt(asml) / s1m;
		} else
			return sqrt(asml) / s1m;
	} else
		return sqrt(amed);
	asml = numext::mini(abig, amed);
	abig = numext::maxi(abig, amed);
	if (asml <= abig * relerr)
		return abig;
	else
		return abig * sqrt(RealScalar(1) + numext::abs2(asml / abig));
}

} // end namespace internal

/** \returns the \em l2 norm of \c *this avoiding underflow and overflow.
 * This version use a blockwise two passes algorithm:
 *  1 - find the absolute largest coefficient \c s
 *  2 - compute \f$ s \Vert \frac{*this}{s} \Vert \f$ in a standard way
 *
 * For architecture/scalar types supporting vectorization, this version
 * is faster than blueNorm(). Otherwise the blueNorm() is much faster.
 *
 * \sa norm(), blueNorm(), hypotNorm()
 */
template<typename Derived>
inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
MatrixBase<Derived>::stableNorm() const
{
	return internal::stable_norm_impl(derived());
}

/** \returns the \em l2 norm of \c *this using the Blue's algorithm.
 * A Portable Fortran Program to Find the Euclidean Norm of a Vector,
 * ACM TOMS, Vol 4, Issue 1, 1978.
 *
 * For architecture/scalar types without vectorization, this version
 * is much faster than stableNorm(). Otherwise the stableNorm() is faster.
 *
 * \sa norm(), stableNorm(), hypotNorm()
 */
template<typename Derived>
inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
MatrixBase<Derived>::blueNorm() const
{
	return internal::blueNorm_impl(*this);
}

/** \returns the \em l2 norm of \c *this avoiding undeflow and overflow.
 * This version use a concatenation of hypot() calls, and it is very slow.
 *
 * \sa norm(), stableNorm()
 */
template<typename Derived>
inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
MatrixBase<Derived>::hypotNorm() const
{
	if (size() == 1)
		return numext::abs(coeff(0, 0));
	else
		return this->cwiseAbs().redux(internal::scalar_hypot_op<RealScalar>());
}

} // end namespace Eigen

#endif // EIGEN_STABLENORM_H
